// includes da biblioteca do joystick + comunicacao serial
#include "joystick.h"
#include "serial/SerialComm.h"
#include "multiwii.h"

// demais bibliotecas nativas
#include <iostream>
#include <unistd.h>

// convencoes de configuracao da comunicao serial/usb
#define COM_PORTA "/dev/ttyACM0"                 // porta de comunicacao USB com Arduino

// convencoes de acesso ao joystick
#define JOYSTICK_PIPE "/dev/input/js0"   			// pipe de leitura do joystick

#define INTERVALO_LEITURA                 500 		// intervalo entre cada leitura do joystick e envio de comando

#define NO_PITCH                          253		// valor para teste lógico que zera o movimento
#define NO_ROLL                           254		// valor para teste lógico que zera o movimento
#define NO_YAW                            251		// valor para teste lógico que zera o movimento
#define NO_THROTTLE                       247		// valor para teste lógico que zera o movimento

#define COMANDO_DESLIGAR					1		// comando para desligar o drone
#define COMANDO_LIGAR 						2		// comando para ligar o drone
#define COMANDO_MOVIMENTO					3		// comando para movimentos do drone

#ifdef __linux__
// macro para acesso a comando de sleep no linux
#include <unistd.h>
#define Sleep(x) usleep(x * 1000);
#endif

using namespace std;

// controle de pressionamento de botoes
// true = pressionado, false = nao pressionado

bool L1 = false;
bool R1 = false;
bool L2 = false;
bool R2 = false;

bool B_0 = false;        	//variáveis para teste lógico de liga e desliga o drone
bool B_2 = false;			//captam os movimentos dos botões 0 e 2

bool AUP = false;           // analógico esquerdo para cima
bool ADOWN = false;         // analógico esquerdo para baixo

// cria instancia do joystick no arquivo '0' (/dev/input/js*)
Joystick joystick(JOYSTICK_PIPE);
// cria instancia de classe comunicacao USB
SerialComm serial(COM_PORTA);

//parâmetros de movimentos a serem enviados
int cmd_UP = 0;
int cmd_DOWN = 0;

unsigned long int bl;

int init() {

	serial.iniciar();

	if (!joystick.isFound()) {
		cout << "Joystick não encontrado.\n" << endl;

		return 1;
	}

	return 0;
}

void sendCmd(int cmd_UP, int cmd_DOWN) {
	serial.escrever(COMANDO_MOVIMENTO, cmd_UP, cmd_DOWN, bl);
	cout << "Enviando comandos. cmd_UP: " << cmd_UP << " cmd_DOWN: " << cmd_DOWN
			<< endl;
}

void executeRC() {
	while (true) {
		// restricao de tempo entre atualizacoes
		Sleep(INTERVALO_LEITURA);

		// captura eventos do joystick
		JoystickEvent event;
		if (joystick.sample(&event)) {
			// verifica os estados dos botoes
			if (event.isButton()) {
				int e = (int) event.number;
				cout << "Botão " << e
						<< (event.value == 0 ?
								" não pressionado" : " pressionado") << endl;

				if ((e == 4) && (event.value)) {
					L1 = true;
				} else if ((e == 4) && (!event.value)) {
					L1 = false;
				}

				if ((e == 5) && (event.value)) {
					R1 = true;
				} else if ((e == 5) && (!event.value)) {
					R1 = false;
				}

				if ((e == 6) && (event.value)) {
					L2 = true;
				} else if ((e == 6) && (!event.value)) {
					L2 = false;
				}

				if ((e == 7) && (event.value)) {
					R2 = true;
				} else if ((e == 7) && (!event.value)) {
					R2 = false;
				}

				//especifica valores de controle para envio do PITCH_UP
				if ((e == 0) && (event.value)) {
					cmd_UP = cmd_UP | (1 << PITCH);
					cout << cmd_UP << endl;
				} else if ((e == 0) && (!event.value)) {
					cmd_UP = cmd_UP & NO_PITCH;
					cout << cmd_UP << endl;
				}

				//especifica valores de controle para envio do ROLL_UP
				if ((e == 1) && (event.value)) {
					cmd_UP = cmd_UP | (1 << ROLL);
					cout << cmd_UP << endl;
				} else if ((e == 1) && (!event.value)) {
					cmd_UP = cmd_UP & NO_ROLL;
					cout << cmd_UP << endl;
				}

				//especifica valores de controle para envio do PITCH_DOWN
				if ((e == 2) && (event.value)) {
					cmd_DOWN = cmd_DOWN | (1 << PITCH);
					cout << cmd_DOWN << endl;
				} else if ((e == 2) && (!event.value)) {
					cmd_DOWN = cmd_DOWN & NO_PITCH;
					cout << cmd_DOWN << endl;
				}

				//especifica valores de controle para envio do ROLL_DOWN
				if ((e == 3) && (event.value)) {
					cmd_DOWN = cmd_DOWN | (1 << ROLL);
					cout << cmd_DOWN << endl;
				} else if ((e == 3) && (!event.value)) {
					cmd_DOWN = cmd_DOWN & NO_ROLL;
					cout << cmd_DOWN << endl;
				}
			}
			// verifica os estados dos controles analogicos
			if (event.isAxis()) {
				int e = (int) event.number;
				int pos = (int) event.value;
				cout << "Controle analógico " << (int) event.number
						<< " na posicao " << (int) event.value << endl;

				// teste dos botões analógicos e a direção
				if ((e == 1) && (pos < 0)) {
					AUP = true;
				} else if ((e == 1) && (pos > 0)) {
					ADOWN = true;
				} else if ((e == 1) && (pos == 0)) {
					AUP = false;
					ADOWN = false;
				}

				if ((e == 3) && (pos < 0)) {
					B_0 = true;
				} else if ((e == 3) && (pos > 0)) {
					B_2 = true;
				} else if ((e == 3) && (pos == 0)) {
					B_0 = false;
					B_2 = false;
				}

				//especifica valores de controle para envio do YAW
				if ((e == 0) && (pos == 32767)) {
					cmd_UP = cmd_UP | (1 << (YAW));
					cout << cmd_UP << endl;
				} else if ((e == 0) && (pos == -32767)) {
					cmd_DOWN = cmd_DOWN | (1 << (YAW));
					cout << cmd_DOWN << endl;
				} else if ((e == 0) && (pos == 0)) {
					cmd_UP = cmd_UP & NO_YAW;
					cmd_DOWN = cmd_DOWN & NO_YAW;
				}

				// especifica valores de controle para envio do THROTTLE
				if (AUP && L1) {
					cmd_UP = cmd_UP | (1 << (THROTTLE));
					cout << cmd_UP << endl;
				} else if (ADOWN && L1) {
					cmd_DOWN = cmd_DOWN | (1 << (THROTTLE));
					cout << cmd_DOWN << endl;
				} else {
					cmd_UP = cmd_UP & NO_THROTTLE;
					cmd_DOWN = cmd_DOWN & NO_THROTTLE;
				}

			}
		}

		if (L1 && R1 && B_0 && AUP) {
			serial.escrever(COMANDO_LIGAR, 0, 0, bl);
			cout << "Ligando Drone." << endl;
		} else if (L2 && R2 && B_2 && ADOWN) {
			serial.escrever(COMANDO_DESLIGAR, 0, 0, bl);
			cout << "Desligando Drone." << endl;
		} else
			sendCmd(cmd_UP, cmd_DOWN);
	}
}

int main(int argc, char** argv) {
	if (init() == 0) {
		executeRC();
	}
}

